#include "SpeedEncoder.h"

namespace Team675
{

SpeedEncoder::SpeedEncoder(UINT8 aModuleNumber, UINT32 aChannel, UINT8 bModuleNumber, UINT32 bChannel) :
		Encoder(aModuleNumber, aChannel, bModuleNumber, bChannel)
{
	m_previousValue = Get();
	m_rateTimer.Reset();
	m_rateTimer.Start();
}

double SpeedEncoder::GetCorrectedRate()
{
	 double t_rate = (Get() - m_previousValue) / m_rateTimer.Get();
	 
	 m_previousValue = Get();
	 m_rateTimer.Reset();
	 m_rateTimer.Start();
	 
	 return t_rate;
}

double SpeedEncoder::PIDGet()
{
	return GetCorrectedRate();
}

}
